import numpy as np
import matplotlib.pyplot as plt

with open("../build/mpc_data.csv", encoding = 'utf-8') as f:
    t = np.loadtxt(f, int, delimiter = ",", skiprows = 1, usecols = (0)) * 0.032

with open("../build/mpc_data.csv", encoding='utf-8') as f:
    pos = np.loadtxt(f, float, delimiter = ",", skiprows = 1, usecols = (1))

with open("../build/mpc_data.csv", encoding='utf-8') as f:
    vel = np.loadtxt(f, float, delimiter = ",", skiprows = 1, usecols = (2))

with open("../build/mpc_data.csv", encoding='utf-8') as f:
    force = np.loadtxt(f, float, delimiter = ",", skiprows = 1, usecols = (3))

with open("../build/mpc_data.csv", encoding='utf-8') as f:
    pos_ref = np.loadtxt(f, float, delimiter = ",", skiprows = 1, usecols = (4))

with open("../build/mpc_data.csv", encoding='utf-8') as f:
    vel_ref = np.loadtxt(f, float, delimiter = ",", skiprows = 1, usecols = (5))


plt.figure(1)

plt.subplot(311)
plt.grid()
plt.plot(t, pos, "b-")
plt.plot(t, pos_ref, "r-")
plt.title("position")
plt.xlabel("t")
plt.ylabel("pos")

plt.subplot(312)
plt.grid()
plt.plot(t, vel, "b-")
plt.plot(t, vel_ref, "r-")
plt.title("velocity")
plt.xlabel("t")
plt.ylabel("m/s")

plt.subplot(313)
plt.grid()
plt.plot(t, force)
plt.title("Force")
plt.xlabel("t")
plt.ylabel("N")

plt.show()
